import copy
import trimesh
import numpy as np
from trimesh.transformations import translation_matrix
from numpy import linalg as LA
from OCC.Core.TopoDS import TopoDS_Shape
from OCC.Core.gp import gp_Pnt, gp_Trsf, gp_Vec, gp_Ax1, gp_Dir
from OCC.Core.BRepBuilderAPI import BRepBuilderAPI_Transform
import detail_design.models.detail_design.jxq.group as group
from OCC.Extend.DataExchange import write_stl_file,read_stl_file
from OCC.Core.BRepAlgoAPI import BRepAlgoAPI_Cut, BRepAlgoAPI_Fuse,BRepAlgoAPI_Common
from OCC.Core.BRep import BRep_Builder
from OCC.Core.TopoDS import TopoDS_Compound
import os


# def apply_transform(shape,matrix):
#     a = [[matrix[i][j] for j in range(4)] for i in range(3)]
#
#     T = gp_Trsf()
#     myshape = TopoDS_Shape(shape)
#     T.SetValues(a[0][0], a[0][1], a[0][2], a[0][3],
#                 a[1][0], a[1][1], a[1][2], a[1][3],
#                 a[2][0], a[2][1], a[2][2], a[2][3])
#     trans = BRepBuilderAPI_Transform(T)
#     trans.Perform(myshape)
#     new_shape = trans.Shape()
#     return new_shape
def apply_transform(shape,matrix):
    a = [[matrix[i][j] for j in range(4)] for i in range(3)]

    T = gp_Trsf()
    myshape = TopoDS_Shape(shape)
    T.SetValues(a[0][0], a[0][1], a[0][2], a[0][3],
                a[1][0], a[1][1], a[1][2], a[1][3],
                a[2][0], a[2][1], a[2][2], a[2][3])
    trans = BRepBuilderAPI_Transform(T)
    trans.Perform(myshape)
    new_shape = trans.Shape()
    return new_shape

def shape_to_mesh(shape):
    filname="temp.stl"
    write_stl_file(shape,filename=filname)
    mesh=trimesh.load_mesh(filname)
    os.remove(filname)
    return mesh


# 夹具的旋转
def model_rotate(obj, angle, axis, original):
    new_obj=copy.deepcopy(obj)
    rotate_matrix = trimesh.transformations.rotation_matrix(angle, axis, original)
    new_obj.apply_transform(rotate_matrix)
    return new_obj

# 夹具的move
def move_clamp_pointTopoint(clamp,point1,point2):
    obj=copy.deepcopy(clamp)
    move_matrix=trimesh.transformations.translation_matrix(np.asarray(point2)-np.asarray(point1))
    obj.apply_transform(move_matrix)
    return obj

# 夹具的move direction
def move_clamp_direction(obj, direction=None):
    new_obj = copy.deepcopy(obj)
    move_matrix = trimesh.transformations.translation_matrix(direction)
    new_obj.apply_transform(move_matrix)
    return new_obj

# # 夹具的沿轴线移动
# def move_along_axis(obj,value=10):
#     new_obj = copy.deepcopy(obj)
#     point1 = obj.get_point("气缸/B.-x")
#     point2 = obj.get_point("气缸/B.+x")
#     direction = np.array(point2 - point1)
#     direction=direction*value/LA.norm(direction)
#     move_matrix = trimesh.transformations.translation_matrix(direction)
#     new_obj.apply_transform(move_matrix)
#     return new_obj

# 夹具的沿轴线移动
def move_along_axis(obj,direction,value=10):
    new_obj = copy.deepcopy(obj)
    direction=direction*value/LA.norm(direction)
    move_matrix = trimesh.transformations.translation_matrix(direction)
    new_obj.apply_transform(move_matrix)
    return new_obj

def update_model_by_history(new_obj,obj):
    new_obj.transform=copy.deepcopy(obj.transform)
    return new_obj

# 检测气缸与工件是否存在碰撞
# def detect_collision(obj=None, env=None,dectect_list=['气缸','支柱']):
#     collision_rigion = trimesh.collision.CollisionManager()
#     jig=None
#     for k,v in obj.elements.items():
#         for test_str in dectect_list:
#             if test_str in k:
#                 m = v['instance'].get_mesh()
#                 m.apply_transform(v['transform'])
#                 if jig is None:
#                     jig=m
#                 else:
#                     jig+=m
#     jig.apply_transform(obj.transform)
#     collision_rigion.add_object(name='tt',mesh=jig)
#     if env:
#             collision_rigion.add_object(name="工件", mesh=env)
#             result = collision_rigion.in_collision_internal()
#             print("Air和工件存在碰撞：", result)
#             return result


def detect_collision(obj=None,env=None,dectect_list=['气缸','支柱']):
    collision_rigion = trimesh.collision.CollisionManager()
    jig=None
    compound = TopoDS_Compound()
    B = BRep_Builder()
    B.MakeCompound(compound)
    for k,v in obj.elements.items():
        for test_str in dectect_list:
            if test_str in k:
                m = v['instance'].compound()
                m=apply_transform(m,v['transform'])
                B.Add(compound,m)

                # return compound
                # if jig is None:
                #     jig=m
                # else:
                #     jig=BRepAlgoAPI_Fuse(jig,m).Shape()


    # from OCC.Display.SimpleGui import init_display
    # display, start_display, add_menu, add_function_to_menu = init_display('wx')
    # display.DisplayShape(compound,update=True)
    # start_display()

    jig=apply_transform(compound,obj.transform)
    jig_mesh=shape_to_mesh(jig)
    collision_rigion.add_object(name='tt',mesh=jig_mesh)
    if env:
            collision_rigion.add_object(name="工件", mesh=env)
            result = collision_rigion.in_collision_internal()
            print("Air和工件存在碰撞：", result)
            return result


def detect_distance_between_mesh(obj,env,dectect_list=['气缸','支柱']):
    collision_rigion = trimesh.collision.CollisionManager()
    jig=None
    for k,v in obj.elements.items():
        for test_str in dectect_list:
            if test_str in k:
                m = v['instance'].get_mesh()
                m.apply_transform(v['transform'])
                if jig is None:
                    jig=m
                else:
                    jig+=m
    jig.apply_transform(obj.transform)
    collision_rigion.add_object(name='tt', mesh=jig)
    distance=collision_rigion.min_distance_single(env)
    print(distance)
    return distance

def detect_distance_between_mesh_and_setting_box(obj_settting_box,env):
    collision_rigion = trimesh.collision.CollisionManager()
    collision_rigion.add_object(name='tt', mesh=obj_settting_box)
    distance=collision_rigion.min_distance_single(env)
    print(distance)
    return distance






